import cv2
import numpy as np

class PyCAN():
    def __init__(self):
        self.bus = None
        self.msg = None
        self.image_X = 1980 ## the length of the image
        self.image_Y = 1080 ## the width of the image

    ## specify  the bus and msg
    def config(self):
        if self.bus is None:
            import can
            self.bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate = 500000)
            self.msg = can.Message(arbitration_id =0x502, data=[0x0, 0x0, 0x0, 0x0], is_extended_id=False)


    """
    " LEDS distribution
    " Left:
    "           L3      L1          L2
    "           L5      L4,L6       L7
    "           L8      L9          L10
    " Right:
    "           R2      R1          R3
    "           R7      R4,R6       R5
    "           R10     R9          R8
    "
    """
    def can_msg_data_generate(self, centers):
        msg_data = [0x00, 0x00, 0x00, 0x00] ## first 2 bytes for Left leds; second 2 bytes for Right leds
        left_leds_distribution = [[[3], [1], [2]], [[5], [4, 6], [7]], [[8], [9], [10]]]
        right_leds_distribution = [[[2], [1], [3]], [[7], [4, 6], [5]], [[10], [9], [8]]]
        for center in centers:
            x, y = center
            pos_x = 6*x//self.image_X
            pos_y = 3*y//self.image_Y

            pos_x = min(pos_x, 5)
            pos_y = min(pos_y, 2)

            if pos_x > 2: ## right
                pos_x -= 3
                for val in right_leds_distribution[pos_y][pos_x]:
                    if val <= 8: ## 3th Byte
                        val = (val - 1) % 8
                        msg_data[2] |= (1 << val)
                    else: ## 4th byte
                        val = (val - 1) % 8
                        msg_data[3] |= (1 << val)
            else: ## left
                for val in left_leds_distribution[pos_y][pos_x]:
                    if val <= 8: ## 1th byte
                        val = (val - 1) % 8
                        msg_data[0] |= (1 << val)
                    else: ## 2th byte
                        val = (val - 1) % 8
                        msg_data[1] |= (1 << val)
        return msg_data


            


    ## send msg according the centers
    def send_msg_according_centers(self, centers):
        if self.msg is None or self.bus is None:
            print("please config the bus at first")
            return
        self.msg.data = self.can_msg_data_generate(centers)
        try:
            self.bus.send(self.msg)
            print("Message sent on {}".format(self.bus.channel_info))
        except can.CanError:
            print("Message NOT send")

    """
    " test function for validation:
    "   - plot the centers in image with image_X and image_Y
    "   - print the position info 
    """
    def plot_centers_in_image(self, centers):

        def center2matrix(centers):
            leds = [[[0], [0], [0], [0], [0], [0]], [[0], [0], [0], [0], [0], [0]], [[0], [0], [0], [0], [0], [0]]]

            for center in centers:
                x, y = center
                pos_x = 6*x//self.image_X
                pos_y = 3*y//self.image_Y

                pos_x = min(pos_x, 5)
                pos_y = min(pos_y, 2)


                leds[pos_y][pos_x] = [1];

            print(leds[0])
            print(leds[1])
            print(leds[2])


        width, height = self.image_X, self.image_Y
        image = np.ones((height, width, 3)) * 255
        line_color = (255, 0, 0)
        line_thickness = 2

        ## 0th vertical
        x1, y1 = width*0//6, 0
        x2, y2 = width*0//6, height-1
        cv2.line(image, (x1, y1), (x2, y2), line_color, thickness=line_thickness)

        ## 1th vertical
        x1, y1 = width*1//6, 0
        x2, y2 = width*1//6, height-1
        cv2.line(image, (x1, y1), (x2, y2), line_color, thickness=line_thickness)

        ## 2th vertical
        x1, y1 = width*2//6, 0
        x2, y2 = width*2//6, height-1
        cv2.line(image, (x1, y1), (x2, y2), line_color, thickness=line_thickness)

        ## 3th vertical
        x1, y1 = width*3//6, 0
        x2, y2 = width*3//6, height-1
        cv2.line(image, (x1, y1), (x2, y2), line_color, thickness=line_thickness)

        ## 4th vertical
        x1, y1 = width*4//6, 0
        x2, y2 = width*4//6, height-1
        cv2.line(image, (x1, y1), (x2, y2), line_color, thickness=line_thickness)

        ## 5th vertical
        x1, y1 = width*5//6, 0
        x2, y2 = width*5//6, height-1
        cv2.line(image, (x1, y1), (x2, y2), line_color, thickness=line_thickness)

        ## 6th vertical
        x1, y1 = width*6//6-1, 0
        x2, y2 = width*6//6-1, height-1
        cv2.line(image, (x1, y1), (x2, y2), line_color, thickness=line_thickness)


        ## 0th vertical
        x1, y1 = 0, height*0//3 
        x2, y2 = width-1, height*0//3
        cv2.line(image, (x1, y1), (x2, y2), line_color, thickness=line_thickness)

        ## 1th vertical
        x1, y1 = 0, height*1//3
        x2, y2 = width-1, height*1//3
        cv2.line(image, (x1, y1), (x2, y2), line_color, thickness=line_thickness)

        ## 2th vertival
        x1, y1 = 0, height*2//3
        x2, y2 = width-1, height*2//3
        cv2.line(image, (x1, y1), (x2, y2), line_color, thickness=line_thickness)

        ## 3th vertical
        x1, y1 = 0, height*3//3
        x2, y2 = width-1, height*3//3
        cv2.line(image, (x1, y1), (x2, y2), line_color, thickness=line_thickness)


        """ draw centers """
        center_radius = 3
        center_color = (0, 0, 255)
        center_thickness = 2

        ## parameters for center text
        font = cv2.FONT_HERSHEY_PLAIN
        fontscale = 3
        color = (0, 0, 0)
        cnt = 0
        for center in centers:
            cnt += 1
            x, y = center
            image = cv2.circle(image, center, center_radius, center_color, center_thickness)
            info2show = "C%d"%(cnt)
            image = cv2.putText(image, info2show, (x-4, y-10), font, fontscale, color, 3)

        ## generate data
        center2matrix(centers)


        image = cv2.resize(image, (self.image_X//3, self.image_Y//3))
        cv2.imshow("image", image)
        cv2.waitKey()
